
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       land_detector.c
  * @author     baiyang
  * @date       2021-8-17
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
/*-----------------------------------macro------------------------------------*/
// Code to detect a crash main ArduCopter code
#define LAND_CHECK_ANGLE_ERROR_DEG  30.0f       // maximum angle error to be considered landing
#define LAND_CHECK_LARGE_ANGLE_CD   1500.0f     // maximum angle target to be considered landing
#define LAND_CHECK_ACCEL_MOVING     3.0f        // maximum acceleration after subtracting gravity
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
// counter to verify landings
static uint32_t land_detector_count = 0;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void fms_update_land_detector()
{
    // land detector can not use the following sensors because they are unreliable during landing
    // barometer altitude :                 ground effect can cause errors larger than 4m
    // EKF vertical velocity or altitude :  poor barometer and large acceleration from ground impact
    // earth frame angle or angle error :   landing on an uneven surface will force the airframe to match the ground angle
    // gyro output :                        on uneven surface the airframe may rock back an forth after landing
    // range finder :                       tend to be problematic at very short distances
    // input throttle :                     in slow land the input throttle may be only slightly less than hover

    if (!fms.motors->_armed) {
        // if disarmed, always landed.
        fms_set_land_complete(true);
    } else if (fms.ap.land_complete) {
        // if throttle output is high then clear landing flag
        if (Motors_get_throttle(fms.motors) > fms_get_non_takeoff_throttle()) {
            fms_set_land_complete(false);
        }
    } else if (fms.standby_active) {
        // land detector will not run in standby mode
        land_detector_count = 0;
    } else {

        // check that the average throttle output is near minimum (less than 12.5% hover throttle)
        bool motor_at_lower_limit = fms.motors->limit.throttle_lower && attctrl_is_throttle_mix_min(fms.attitude_control);

        uint8_t land_detector_scalar = 1;

        // check that the airframe is not accelerating (not falling or braking after fast forward flight)
        bool accel_stationary = (vec3_length(&fms.land_accel_ef_filter.output) <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);

        // check that vertical speed is within 1m/s of zero
        bool descent_rate_low = fabsf(fms.ahrs->velocity_cm.z) < 100 * land_detector_scalar;

        // if we have a healthy rangefinder only allow landing detection below 2 meters
        //bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);

        // if we have weight on wheels (WoW) or ambiguous unknown. never no WoW
        const bool WoW_check = true;

        if (motor_at_lower_limit && accel_stationary && descent_rate_low && WoW_check) {
            // landed criteria met - increment the counter and check if we've triggered
            if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*0.005) {
                land_detector_count++;
            } else {
                fms_set_land_complete(true);
            }
        } else {
            // we've sensed movement up or down so reset land_detector
            land_detector_count = 0;
        }
    }

    fms_set_land_complete_maybe(fms.ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*0.005));
}

// set land_complete flag and disarm motors if disarm-on-land is configured
void fms_set_land_complete(bool b)
{
    // if no change, exit immediately
    if( fms.ap.land_complete == b )
        return;

    land_detector_count = 0;

#if 0
    if(b){
        AP::logger().Write_Event(LogEvent::LAND_COMPLETE);
    } else {
        AP::logger().Write_Event(LogEvent::NOT_LANDED);
    }
#endif

    fms.ap.land_complete = b;

    // tell AHRS flying state
    //set_likely_flying(!b);
    
    // trigger disarm-on-land if configured
    bool disarm_on_land_configured = (fms.g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
    const bool mode_disarms_on_land = fms.flightmode->allows_arming(ARMING_CHECK_LANDING) && !fms.flightmode->has_manual_throttle();

    if (fms.ap.land_complete && fms.motors->_armed && disarm_on_land_configured && mode_disarms_on_land) {
        fms.arming.ops->disarm(&fms.arming, ARMING_CHECK_LANDED, true);
    }
}

// set land complete maybe flag
void fms_set_land_complete_maybe(bool b)
{
    // if no change, exit immediately
    if (fms.ap.land_complete_maybe == b)
        return;
#if 0
    if (b) {
        AP::logger().Write_Event(LogEvent::LAND_COMPLETE_MAYBE);
    }
#endif

    fms.ap.land_complete_maybe = b;
}

// run land and crash detectors
// called at MAIN_LOOP_RATE,200Hz
void fms_update_land_and_crash_detectors()
{
    // update 1hz filtered acceleration
    Vector3f_t accel_ef = fms.ahrs->accel_ef;
    accel_ef.z += GRAVITY_MSS;
    lpf_apply2_vec3f(&fms.land_accel_ef_filter, &accel_ef, 0.005);

    fms_update_land_detector();

    //crash_check();
    //thrust_loss_check();
    //yaw_imbalance_check();
}

// TODO: 位置控制
// sets motors throttle_low_comp value depending upon vehicle state
//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
//  has no effect when throttle is above hover throttle
void fms_update_throttle_mix()
{
    // if disarmed or landed prioritise throttle
    if (!fms.motors->_armed || fms.ap.land_complete) {
        attctrl_set_throttle_mix_min(fms.attitude_control);
        return;
    }

    if (fms.flightmode->has_manual_throttle()) {
        // manual throttle
        if (fms.channel_throttle->control_in <= 0 || fms.air_mode == AIRMODE_DISABLED) {
            attctrl_set_throttle_mix_min(fms.attitude_control);
        } else {
            attctrl_set_throttle_mix_man(fms.attitude_control);
        }
    } else {
        // autopilot controlled throttle

        // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
        const Vector3f_t angle_target = attctrl_get_att_target_euler_cd(fms.attitude_control);
        bool large_angle_request = (math_norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);

        // check for large external disturbance - angle error over 30 degrees
        const float angle_error = attctrl_get_att_error_angle_deg(fms.attitude_control);
        bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);

        // check for large acceleration - falling or high turbulence
        const bool accel_moving = (vec3_length(&fms.land_accel_ef_filter.output) > LAND_CHECK_ACCEL_MOVING);

        // check for requested descent
        //bool descent_not_demanded = pos_control->get_vel_desired_cms().z >= 0.0f;

        // check if landing
        const bool landing = fms.flightmode->is_landing();

        if ((large_angle_request && !landing) || large_angle_error || accel_moving) {
            //attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio());
        } else {
            attctrl_set_throttle_mix_min(fms.attitude_control);
        }
    }
}

/*------------------------------------test------------------------------------*/


